// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: 2022 Ivan Baidakou

#include "common.h"
#include <rotor-light.hpp>
#include <avr/interrupt.h>
#include <avr/io.h>
#include <avr/sleep.h>
#include <avr/wdt.h>
#include <util/delay.h>

#define LED PB5

namespace rl = rotor_light;

namespace message {
struct Ping : rl::Message {
  using Message::Message;
  static constexpr auto type_id = __LINE__;
  rl::MessageTypeId get_type_id() const override { return type_id; }
};

struct Pong : rl::Message {
  using Message::Message;
  static constexpr auto type_id = __LINE__;
  rl::MessageTypeId get_type_id() const override { return type_id; }
};
} // namespace message

struct Pinger : rl::Actor<2> {
  using Parent = Actor<2>;

  void initialize() override {
    subscribe(&Pinger::on_pong);
    Parent::initialize();
  }

  void advance_start() override {
    Parent::advance_start();
    ping();
  }

  void ping() {
    /* toggle led */
    PORTB ^= (1 << LED);
    send<message::Ping>(0, ponger_id);
  }

  void on_pong(message::Pong &) {
    add_event(
        500, [](void *data) { static_cast<Pinger *>(data)->ping(); }, this);
  }

  rl::ActorId ponger_id;
};

struct Ponger : rl::Actor<2> {
  using Parent = Actor<2>;

  void initialize() override {
    subscribe(&Ponger::on_ping);
    Parent::initialize();
  }
  void on_ping(message::Ping &) {
    //
    send<message::Pong>(0, pinger_id);
  }
  rl::ActorId pinger_id;
};

using Supervisor =
    rl::Supervisor<rl::SupervisorBase::min_handlers_amount, Pinger, Ponger>;
using Storage = rl::traits::MessageStorage<rl::message::ChangeState,
                                           rl::message::ChangeStateAck,
                                           message::Ping, message::Pong>;
using Queue = rl::Queue<Storage, 5>; /* upto 5 messages in 1 queue */
using Planner = rl::Planner<1>;      /* upto 1 time event */

static void app_hw_init();
static void perform_sleep(const rl::TimePoint &until);

int main(int, char **) {
  app_hw_init();

  /* allocate */
  Queue queue;
  Planner planner;
  rl::Context context{&queue, &planner, &get_now};
  Supervisor sup;

  /* setup */
  sup.bind(context);
  auto pinger = sup.get_child<0>();
  auto ponger = sup.get_child<1>();
  pinger->ponger_id = ponger->get_id();
  ponger->pinger_id = pinger->get_id();
  /* let it polls timer */
  sup.start(false);

  /* main cycle */
  while (true) {
    sup.process();
    auto next_event_time = planner.next_event();
    if (next_event_time) {
      perform_sleep(next_event_time);
    }
  }
  return 0;
}

uint8_t mcusr_mirror __attribute__((section(".noinit")));
void get_mcusr(void) __attribute__((naked)) __attribute__((section(".init3")));
void get_mcusr(void) {
  mcusr_mirror = MCUSR;
  MCUSR = 0;
  wdt_disable();
}

void perform_sleep(const rl::TimePoint &until) {
  auto now = get_now();
  auto left = until - now;
  auto amount = rl::Duration{};
  auto sleep_constant = uint8_t{};

  cli();
  disable_timer();
  while (left > 0) {
    cli();
    if (left < 15) {
      break;
    } else if (left < 30) {
      amount = 15;
      sleep_constant = WDTO_15MS;
    } else if (left < 60) {
      amount = 30;
      sleep_constant = WDTO_30MS;
    } else if (left < 120) {
      amount = 60;
      sleep_constant = WDTO_60MS;
    } else if (left < 250) {
      amount = 120;
      sleep_constant = WDTO_120MS;
    } else if (left < 500) {
      amount = 250;
      sleep_constant = WDTO_250MS;
    } else if (left < 1000) {
      amount = 500;
      sleep_constant = WDTO_500MS;
    } else if (left < 2000) {
      amount = 1000;
      sleep_constant = WDTO_1S;
    } else if (left < 4000) {
      amount = 2000;
      sleep_constant = WDTO_2S;
    } else if (left < 8000) {
      amount = 4000;
      sleep_constant = WDTO_4S;
    } else {
      amount = 8000;
      sleep_constant = WDTO_8S;
    }
    TCNT0 = 0;
    adjust_timer(amount * 1000);
    wdt_reset();
    wdt_enable(sleep_constant);
    // enable watchdog interrupt
    WDTCSR |= (1 << WDIE);

    set_sleep_mode(SLEEP_MODE_PWR_DOWN);
    sleep_enable();
    sei();
    sleep_cpu();
    sleep_disable();
    sei();
    left -= amount;
  }
  enable_timer();
  sei();
}

static void app_hw_init() {
  // We will now disable the watchdog.
  // Service the watchdog just to be sure to avoid pending timeout.
  wdt_reset();

  // Clear WDRF in MCUSR.
  MCUSR &= ~(1U << WDRF);

  // Write logical one to WDCE and WDE.
  // Keep the old prescaler setting to prevent unintentional time-out.
  WDTCSR |= (1U << WDCE) | (1U << WDE);

  // Turn off the WDT.
  WDTCSR = 0x00;

  // We will now initialize PORTB.5 to be used as an LED driver port.
  // Set PORTB.5 value to low.
  PORTB &= ~(1U << PORTB5);

  // Set PORTB.5 direction to output.
  DDRB |= (1U << DDB5);

  enable_timer();

  // Enable all interrupts.
  sei();
}

ISR(WDT_vect) {
  // WDIE & WDIF is cleared in hardware upon entering this ISR
  wdt_disable();
}
